/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-03-01     86188       the first version
 */
#include "head_rtthread.h"
#include "head_user.h"

static void Stabilize_Contral(IMU_st *imu, Axis3f *gyro);
static void Altitude_Control(void);
static void Position_Control(void);
/*===================================================================*/
/* 主飞行任务 */
/*===================================================================*/
void fly_Task(void* parameter)
{
    PID_Handle.All_PidInit();

    static uint32_t tick;
    IMU_st *imu     = INS_Handle.imuAngle;
    Axis3f *gyro    = &IMU_Handle.imu_data->gyro;
    PID_Struct *pid = PID_Handle.pid;

    /* pid supplement_init */
    pid[Rol_Angle].dt = 1.0/500;
    pid[Pit_Angle].dt = 1.0/500;
    pid[Yaw_Angle].dt = 1.0/500;

    pid[Rol_Rate].dt = 1.0/500;
    pid[Pit_Rate].dt = 1.0/500;
    pid[Yaw_Rate].dt = 1.0/500;

    pid[Z_Position].dt  = 1.0/250;
    pid[Z_Speed].dt     = 1.0/250;
    pid[X_Position].dt  = 1.0/100;
    pid[Y_Position].dt  = 1.0/100;
    pid[X_Speed].dt     = 1.0/100;
    pid[Y_Speed].dt     = 1.0/100;
    while(1)
    {
        if(tick%5 == 0)
        {
            Position_Control();         //100HZ
        }
        if(tick%2 == 0)
        {
            Altitude_Control();         //250HZ
        }
        Stabilize_Contral(imu, gyro);   //500HZ

        tick++;
        rt_thread_mdelay(2);
    }
}



static void Stabilize_Contral(IMU_st *imu, Axis3f *gyro)
{
    PID_Struct *pid = PID_Handle.pid;

    /***********************偏航角度控制****************************/
    if( RC_Handle.rc_dat[YAW] == 0 )                                                         //偏航杆置于中位
    {
        if(pid[Yaw_Angle].Expect == 0)                                                //摇杆刚回中的一刻锁定锁定航向角
        {
            pid[Yaw_Angle].Expect = imu->yaw;
        }
        if( RC_Handle.rc_dat[THR] > -400)//油门大于一定值才启用偏航控制
        {
            pid[Yaw_Rate].Expect = PID_Handle.Yaw_angle_Updata(&pid[Yaw_Angle], imu->yaw);       //角度环控制
        }

    }
    else                                                                            //波动偏航方向杆后，只进行内环角速度控制,其角速度期望来自摇杆
    {
         pid[Yaw_Rate].Expect  = -RC_Handle.rc_dat[YAW]*0.3f;                                  //偏航角速度环期望，直接来源于遥控器打杆量
         pid[Yaw_Angle].Expect = 0;                                                   //偏航角期望给0,不进行角度控制
    }
    PID_Handle.Update(&pid[Yaw_Rate], -gyro->z);

    /***********************横滚俯仰角度角度控制****************************/
    /* 判断角度期望来源 */
    if( RC_Handle.Flyflag.Position )                              //如果使能了定点模式，自稳角度期望来自于位置pid输出
    {
        pid[Rol_Angle].Expect = limit(pid[X_Speed].OutPut*( 0.06f), -30, 30);
        pid[Pit_Angle].Expect = limit(pid[Y_Speed].OutPut*(-0.06f), -30, 30);
    }
    else                                                                    //自稳角度期望来自于摇杆输出
    {
        pid[Rol_Angle].Expect = RC_Handle.datswitch(RC_Handle.rc_uint[ROL], 30);
        pid[Pit_Angle].Expect = RC_Handle.datswitch(RC_Handle.rc_uint[PIT], 30);
    }
    pid[Rol_Rate].Expect = PID_Handle.Update(&pid[Rol_Angle], 0);        //横滚角速度环期望,来源于横滚角度控制器输出
    pid[Pit_Rate].Expect = PID_Handle.Update(&pid[Pit_Angle], 0);        //俯仰角速度环期望,来源于俯仰角度控制器输出

    /***************角速度控制，角速度的期望来自角度pid的输出*****************/
    PID_Handle.Update(&pid[Rol_Rate] , 0);
    PID_Handle.Update(&pid[Pit_Rate] , 0);
}
static void Altitude_Control(void)   //非自主模式下  摇杆控制上升下降速度的函数
{
    PID_Struct *pid = PID_Handle.pid;

    if( RC_Handle.Flyflag.Altitude ) /*如果是定高模式 则进行高度控制*/
    {
        if( RC_Handle.rc_dat[THR] == 0 )       //定点模式,油门为零才会锁定期望
        {
            /* 刚从摇杆控制释放 */
            if( pid[Z_Position].Expect == 0 )                  //锁定在定点模式下控制刚回中一刻的高度
            {
                pid[Z_Position].Expect = BODY.nfPos.z;
            }
            pid[Z_Speed].Expect = PID_Handle.Update(&pid[Z_Position], BODY.nfPos.z);     //更新内环期望
            PID_Handle.Update(&pid[Z_Speed], BODY.nfSpeed.z);
        }
        else  //给定上升/下降速度期望
        {
            pid[Z_Position].Expect = 0;
            pid[Z_Speed].Expect    = RC_Handle.datswitch(RC_Handle.rc_uint[THR], 200);
            PID_Handle.Update(&pid[Z_Speed], BODY.nfSpeed.z);
        }
    }
    else   /*不是定高模式时  清空高度控制pid*/
    {
        PID_Handle.pidReset(Z_Position);
        PID_Handle.pidReset(Z_Speed);
    }
}
//static void Position_Control(void)
//{
//    PID_Struct *pid = PID_Handle.pid;
//
//    if( RC_Handle.Flyflag.Position )
//    {
//        /* x方向无遥控量 */
//        if( RC_Handle.rc_dat[ROL] == 0 )
//        {
//            if( pid[X_Position].Expect==0 )
//            {
//                pid[X_Position].Expect = BODY.Pos.x;
//            }
//            pid[X_Speed].Expect = PID_Handle.Update(&pid[X_Position], BODY.Pos.x);            //更新内环期望
//
//            PID_Handle.Update(&pid[X_Speed], BODY.Speed.x);      //仅在定点时启用加速度补偿
//        }
//        else
//        {
//            pid[X_Position].Expect  = 0;                                                            //横滚位置期望复位
//            pid[X_Speed].Expect     = RC_Handle.datswitch(RC_Handle.rc_uint[ROL], 200);
//            PID_Handle.Update(&pid[X_Speed], BODY.Speed.x);
//        }
//        /* y方向无遥控量 */
//        if( RC_Handle.rc_dat[PIT] == 0 )
//        {
//            if( pid[Y_Position].Expect == 0 )
//            {
//                pid[Y_Position].Expect = BODY.Pos.y;
//            }
//            pid[Y_Speed].Expect = PID_Handle.Update(&pid[Y_Position], BODY.Pos.y);
//            PID_Handle.Update(&pid[Y_Speed], BODY.Speed.y);
//        }
//        else
//        {
//            pid[Y_Position].Expect  = 0;
//            pid[Y_Speed].Expect     = -RC_Handle.datswitch(RC_Handle.rc_uint[PIT], 200);
//            PID_Handle.Update(&pid[Y_Speed], BODY.Speed.y);
//        }
//    }
//    else
//    {
//        PID_Handle.pidReset(Y_Position);
//        PID_Handle.pidReset(X_Position);
//        PID_Handle.pidReset(Y_Speed);
//        PID_Handle.pidReset(X_Speed);
//    }
//}

static void Position_Control(void)
{
    PID_Struct *pid = PID_Handle.pid;
    Axis2f nf, bf;
    static uint8_t lock_x, lock_y;

    if( RC_Handle.Flyflag.Position && RC_Handle.Flyflag.BodyState == unlock)
    {
        /* lock location when remote have no contral */
        if( RC_Handle.rc_dat[ROL] == 0 && lock_x == 0 )
        {
            lock_x = 1;
            pid[X_Position].Expect = BODY.nfPos.x;
        }
        if( RC_Handle.rc_dat[PIT] == 0 && lock_y == 0 )
        {
            lock_y = 1;
            pid[Y_Position].Expect = BODY.nfPos.y;
        }
        /* calculate the error of body frame */
        nf.x = pid[X_Position].Expect - BODY.nfPos.x;
        nf.y = pid[Y_Position].Expect - BODY.nfPos.y;
        BODY.Navi_To_Body_2(&bf, &nf, -INS_Handle.imuAngle->yaw);

        if ( RC_Handle.rc_dat[ROL] == 0 )
        {
            pid[X_Speed].Expect = PID_Handle.error_Updata(&pid[X_Position], bf.x);
            PID_Handle.Update(&pid[X_Speed], BODY.bfSpeed.x);
        }
        else {
            lock_x = 0;
            pid[X_Speed].Expect = RC_Handle.datswitch(RC_Handle.rc_uint[ROL], 200);
            PID_Handle.Update(&pid[X_Speed], BODY.bfSpeed.x);
        }
        if ( RC_Handle.rc_dat[PIT] == 0 )
        {
            pid[Y_Speed].Expect = PID_Handle.error_Updata(&pid[Y_Position], bf.y);
            PID_Handle.Update(&pid[Y_Speed], BODY.bfSpeed.y);
        }
        else {
            lock_y = 0;
            pid[Y_Speed].Expect = -RC_Handle.datswitch(RC_Handle.rc_uint[PIT], 200);
            PID_Handle.Update(&pid[Y_Speed], BODY.bfSpeed.y);
        }
    }
    else
    {
        PID_Handle.pidReset(Y_Position);
        PID_Handle.pidReset(X_Position);
        PID_Handle.pidReset(Y_Speed);
        PID_Handle.pidReset(X_Speed);
    }
}
